﻿using System;
using System.Linq;
using System.Text;
using System.Collections;
using Aldebaran.Proxies;

namespace grounding.nao.sensors
{
    public class Sonar : ISensor
    {
        private SonarProxy sonarProxy;
        private MemoryProxy memoryProxy;
        private string name;

        /// <summary>
        /// Returns the name of the Sonar sensor.  Possible values include:
        /// <list type="bullet">
        /// <item>sonar</item>
        /// </list>
        /// </summary>
        public string Name { get { return name; } }

        /// <summary>
        /// Returns the string "sonar" as the identifier for this sensor type
        /// </summary>
        public string Type { get { return "sonar"; } }

        /// <summary>
        /// Assumes proxies are connected to robot.
        /// </summary>
        /// <param name="sonarProxy">established SonarProxy</param>
        /// <param name="memoryProxy">established MemoryProxy</param>
        public Sonar(SonarProxy sonarProxy, MemoryProxy memoryProxy, string name)
        {
            this.sonarProxy = sonarProxy;
            this.memoryProxy = memoryProxy;
            this.name = name;
        }

        /// <summary>
        /// Polls the sonar for the current range.
        /// 
        /// NOTE: Effective range = 15 - 70 cm.
        /// </summary>
        /// <returns>A SonarSensorFrame of the current sonar range.</returns>
        public ISensorFrame GetSensorFrame() { return GetSensorFrame(x => x); }
        /// <summary>
        /// Polls the sonar for the current range.
        /// 
        /// NOTE: Effective range = 15 - 70 cm.
        /// </summary>
        /// <returns>A SonarSensorFrame of the current sonar range.</returns>
        public ISensorFrame GetSensorFrame(Func<System.Collections.Generic.IEnumerable<double>, System.Collections.Generic.IEnumerable<double>> transformer)
        {
            // Prepare list of keys.
            ArrayList keys = new ArrayList();
            keys.Add("Device/SubDeviceList/US/Left/Sensor/Value");
            keys.Add("Device/SubDeviceList/US/Right/Sensor/Value");

            // Fetch the data.
            ArrayList rawValues = (ArrayList)memoryProxy.getListData(keys);

            // Convert to an array of doubles.
            double[] values = new double[1];
            values[0] = ((float)rawValues[0] + (float)rawValues[1]) / 2;

            return new SonarSensorFrame(this, transformer(values).ToArray());
        }
    }
}
